'Inspire OS
' - For the Virginia Beach Robotics Competition
'Copyright (c) 2012, Robert Schofield and James Taylor

'This is our template. These will be elaborated on and some portions might
'be removed. No relevant code has been added yet.

CON
  ServoCh1 = 16     ' CHANGE THESE TO MAKE STUFF EASIER, LATER. *TODO*
  ServoCh2 = 17
  ServoCh3 = 18
  ServoCh4 = 19
  STP      = 1500

  LFWD     = 2000
  RFWD     = 1000
  LRVRS    = 1000
  RRVRS    = 2000

  FORWARD  = 1
  REVERSE  = 2
  LEFT     = 3
  RIGHT    = 4
  STOP     = 5
  
VAR
  byte state

OBJ
  pst : "Parallax Serial Terminal"
  SERVO : "Servo32v7.spin"
  
PUB initialize
  pst.Start(115200)
  servo.Start
  servo.Ramp
  
PUB LED(pin, OnOff)
  dira[pin] := outa[pin] := OnOff
  repeat

PUB RecvState(pin)
  state := ina[pin]                        'When run, the output should look like the following in the 
  pst.str(string("State of pin "))         'Parallax Serial Terminalwhen pin = 16 and the state = 1:
  pst.str(pin)                             'State of pin 16: 1
  pst.str(string(" : "))
  pst.str(state)
  pst.str(string(13))

PUB wait(time)                             'sets 'time' as referenced variable
    waitCnt(clkfreq/time + cnt)            'places variable 'time' in standard pause function

PUB Drive(Direction)             ' Syntax: Drive( Direction to Drive in)  Options: FORWARD, REVERSE, RIGHT, LEFT, STOP
  if (direction == FORWARD)
      servo.Set(ServoCh1,LFWD)
      servo.Set(ServoCh2,LFWD)
      servo.Set(ServoCh3,RFWD)
      servo.Set(ServoCh4,RFWD)    
  if (direction == REVERSE)
      servo.Set(ServoCh1,LRVRS)
      servo.Set(ServoCh2,LRVRS)
      servo.Set(ServoCh3,RRVRS)
      servo.Set(ServoCh4,RRVRS)  
  if (direction == RIGHT)
      servo.Set(ServoCh1,LFWD)
      servo.Set(ServoCh2,LFWD)
      servo.Set(ServoCh3,RRVRS)
      servo.Set(ServoCh4,RRVRS)
  if (direction == LEFT)
      servo.Set(ServoCh1,LRVRS)
      servo.Set(ServoCh2,LRVRS)
      servo.Set(ServoCh3,RFWD)
      servo.Set(ServoCh4,RFWD)
  if (direction == STOP)
      servo.Set(ServoCh1,STP)
      servo.Set(ServoCh2,STP)
      servo.Set(ServoCh3,STP)
      servo.Set(ServoCh4,STP)

      